import java.util.Vector;

import clientAPI.*;
import robotAPI.*;

public class ThreadCommunication {
	Vector<ServerMessage> robotMessages = new Vector<ServerMessage>();
	Vector<ClientMessage> clientMessages = new Vector<ClientMessage>();
	
	Map map;
	
	public ThreadCommunication(){
		map = new Map(400, 280, this);
	}
	
	public RobotRequestMessage getRobotRequestMessage(){
		RobotRequestMessage rReqMsg = null;
		if (clientMessages.size() > 0){
			ClientMessage msg = clientMessages.firstElement();
			if (msg.automatic())
				rReqMsg = new RobotRequestMessage(true);
			else if (msg.getDrive() == null)
				rReqMsg = new RobotRequestMessage(msg.getMove(), msg.getRotate(), msg.getNumScans(), false);
			else
				rReqMsg = new RobotRequestMessage(msg.getDrive(), msg.getTurn());
			clientMessages.removeElementAt(0);
		}
		return rReqMsg;
	}
	
	public ServerMessage getServerMessage(){
		ServerMessage sMsg = null;
		if (robotMessages.size()>0){
			sMsg = robotMessages.firstElement();
			robotMessages.removeElementAt(0);
		}		
		return sMsg;
	}
	
	public void addClientMessage(ClientMessage msg){
		clientMessages.add(msg);
	}
	
	public void addServerMessage(ServerMessage msg){
		robotMessages.add(msg);
	}
	
	public void addRobotReplyMessage(RobotReplyMessage msg){
		if (msg.hasScanValues())
			processMessage(msg);
	}
	
	void processMessage(RobotReplyMessage msg){
		map.moveRobot(msg.getMoveReal(), msg.getRotateReal());
		int nScan = msg.getNumScan();
		for (int i = 0;i<nScan;i++){
			int distance = msg.getScanValueN(i);
			int angle = i*(int)(360/nScan);
			if (distance < 50){
				System.out.println("obst dist: "+distance+ " angle: " + angle);
				map.addObstacleRelRobot(distance, angle);
			}
		}
		map.resendRobotPosition();
	}
}
